from mealpy import FloatVar, SCA
from SAGIN_Progrem.entity.Appliances import Appliances
from SAGIN_Progrem.entity.UAV import UAV
import matplotlib.pyplot as plt
from SAGIN_Progrem.ENV import Env

app_list = []  # 所有传感器的集合
uav_list = []  # 所有无人机的集合
app_coor_list = [[168.9992961011875, 148.77895793793846], [68.79043691030662, 252.01596025763644],
                 [264.76915322612126, 209.29275565258146], [261.1465715929035, 36.014262358533024],
                 [96.39899064978124, 118.46986226387428], [232.2709682792418, 251.8932971049062],
                 [299.7663059222159, 163.5641596793262], [229.77326425584062, 245.12133433888073],
                 [27.510532284273392, 117.86056453234374], [21.619683197958583, 156.26054774622455],
                 [5.05526041924681, 151.26200934871733], [120.43469583917496, 33.149867010878296],
                 [126.63886047357316, 258.07076842632245], [65.71413606346364, 22.686471888603432],
                 [194.49439170605748, 255.0522925325961], [146.55134956880653, 31.87917115359964],
                 [142.47274685087547, 156.96636727776374], [188.927588721916, 244.5129777909142],
                 [191.6970645083724, 228.36418295194176], [39.48560765342896, 222.23073225625606],
                 [210.06974231068594, 210.39480347358813], [154.88730257547732, 277.5866408969766],
                 [151.24765003362302, 58.81708212116587], [7.6601971488984955, 153.52958144591457],
                 [59.40839657358934, 254.37337193266717], [89.2579290959439, 138.53200847983715],
                 [276.9323069371984, 58.20880662711577], [232.66315130096243, 253.1671773614607],
                 [209.78253176171398, 37.35044597224568], [81.46514189899601, 39.58962878468998],
                 [34.94112347806313, 273.68193821567576], [50.323200696752494, 101.30017819944817],
                 [61.3221289269547, 186.40831133767102], [131.35513615387086, 113.50936864807198],
                 [241.2715331813307, 241.97149349587437], [40.47408622180559, 255.72306340257285],
                 [128.33871462731088, 268.8624647146435], [190.24010038165935, 259.9842346147468],
                 [137.14993650882414, 72.77674273656254], [3.374209878456269, 268.4308068376326],
                 [175.08856869768024, 33.77471847713842], [142.2893786453823, 200.3671943455782],
                 [91.90813237028263, 247.3035455458755], [86.19656090111782, 159.29236934002185],
                 [61.078597861943805, 51.72919810179859], [83.60173922286887, 299.3268409521438],
                 [206.10568844693492, 141.2920667894033], [26.965228541599974, 158.9550955068711],
                 [54.781834993523226, 233.75481073890737], [91.4052320943551, 113.17241745255193],
                 [195.73841740990542, 127.32538040009116], [161.3340724580552, 69.14763219881563],
                 [73.41449435386295, 2.8189752534782175], [19.473335030513837, 164.42042247914773],
                 [63.10765687415796, 44.919532234519096], [118.94422535764289, 3.7069236500316904],
                 [161.75157408235367, 210.32115429882612], [148.4442220823616, 268.2183492833107],
                 [56.29052697884179, 160.16110590959008], [203.54206185490835, 196.0746387775393],
                 [194.3524998790222, 192.68316956843486], [250.1992815097888, 202.396104689441],
                 [103.0103926381177, 74.81238636315437], [23.71485806910019, 161.01405760218043],
                 [87.49595885243873, 286.8137471156991], [76.32342334878507, 245.91475616753868],
                 [84.9548540672657, 144.93815623255676], [191.4941338962086, 22.888490462906052],
                 [33.02245428885744, 229.877878008148], [100.13484061282351, 292.27495128871647],
                 [105.36994787433433, 135.36872110221745], [208.47469673490372, 172.9968018121175],
                 [237.6946852327302, 248.07601736444445], [201.35861191858882, 220.2806811757061],
                 [24.07961602240263, 134.2469388511351], [148.6396111691499, 236.0584191625767],
                 [184.8000595004617, 36.731020109407396], [238.46952237267726, 262.87169434412505],
                 [157.25563988891494, 200.39330799305603], [113.2589202748508, 42.43924821194991],
                 [245.58212700309005, 294.6377369384179], [39.07991633407354, 216.580914928213],
                 [176.62756594655644, 33.55260340816518], [65.86965408715344, 199.81011643461397],
                 [161.1502139311998, 257.4472184146118], [10.474254729837762, 191.7179086228613],
                 [202.88370329827293, 191.44991278527135], [287.45745272222666, 140.18309090719487],
                 [280.11344905322113, 28.29694181208594], [168.3167007159406, 168.98483106418064],
                 [21.603960620860796, 132.8968543331693], [109.43483133086451, 90.88383552950202],
                 [164.6840244022289, 4.435325581043836], [64.36968171431504, 299.856180400684],
                 [58.2187147154449, 294.7651477956747], [148.66462768588224, 242.68534464784727],
                 [103.75704019872792, 286.4709942217844], [180.96612095000677, 150.93252878147098],
                 [37.66549036626168, 68.60830532529687], [86.91434016361832, 181.89243425699232],
                 [278.43000515512347, 162.63948632065623], [241.44657487133247, 94.61381176032388],
                 [5.443560380843914, 269.4038742469291], [113.78768731072454, 279.7898034375263],
                 [153.63284732792195, 214.35531875788075], [85.36149032434756, 42.34321254477656],
                 [235.9479921676705, 286.28273865137095], [193.36200650346348, 286.77048650789254],
                 [154.81527493475093, 42.74931800555181], [136.46822045006783, 168.14626665954526],
                 [160.8186975321746, 162.1011450091756], [51.611881122772665, 110.98467246851047],
                 [100.32886864963444, 180.3411048339076], [131.27587098232922, 151.79136974039324],
                 [72.89119492811416, 240.6359968853185], [281.65942914171904, 41.21558313579378],
                 [113.70437540600888, 269.1086750957092], [26.335366762135457, 63.50735044424234],
                 [100.70558775811095, 267.62435778256076], [276.11453959858255, 60.22228077472415],
                 [102.57598740304383, 52.165456498941396], [131.24553550861873, 2.901980774399715],
                 [85.36508411475802, 193.67255222039756], [259.3936301071414, 280.9827011347498],
                 [168.31309334414055, 7.914667471976278], [228.0883856926307, 181.98330045763777],
                 [84.60839192400425, 50.1529654297872], [136.1917460358806, 220.58211704591108],
                 [45.720228719050525, 236.69041036652183], [267.08091130656186, 188.2087823005201],
                 [2.954318776389153, 260.85208992169566], [32.28309761587561, 257.5839570344942],
                 [201.1670373259422, 62.215153572947266], [126.97823694941535, 122.77355997836993],
                 [256.1281995191852, 44.42696669332237], [169.39568451067336, 269.10363828714657],
                 [199.8801857528227, 21.375916537462036], [41.229364872414834, 262.98399840083243],
                 [176.7357840928191, 110.5901246179181], [153.1884859365004, 87.73814963578258],
                 [85.79410540287333, 86.64614205668461], [112.79527502764982, 177.4658280726565],
                 [170.47972068678087, 30.151263998694887], [119.87406438179511, 145.18300097487528],
                 [42.07639748591535, 181.85463854120013], [93.30297244796319, 216.81593769085455],
                 [29.227806981469207, 257.6681771085724], [40.033314838487456, 16.518476825859928],
                 [238.40725946323445, 34.41429991059246], [277.2250772597501, 131.15297299306414],
                 [56.79362063292721, 165.51009376223965], [231.4370348515523, 180.6191891653702],
                 [244.7517452658674, 46.063411110258976], [174.21565239099715, 109.68247455118004],
                 [261.8342406178962, 251.78690450492752], [177.75458144443868, 184.56969567027073],
                 [215.0671685575112, 83.55381728516562], [232.10083789614174, 205.5902529557039],
                 [89.15831154592449, 3.1528997780341794], [167.93386765801856, 163.75423823316314],
                 [115.04243658893324, 70.57370574215459], [114.28176139172595, 258.19669795196864],
                 [7.390618382456027, 278.08647121577917], [27.943737165430406, 21.029115107951945],
                 [250.89003946168407, 100.98354919057674], [161.49561746467015, 266.9527440272872],
                 [166.12002058362637, 149.92607991492102], [269.12879568725737, 104.83254997154965],
                 [233.96073685254407, 224.17582253798025], [206.2203168496142, 155.48355879530686],
                 [16.120416588431674, 111.85182323340945], [82.28022794927881, 248.01425239065406],
                 [127.60629775564311, 1.22964992892034], [71.87601721613237, 268.45809517507394],
                 [148.9662474609474, 53.0574994397823], [167.89360469595672, 192.4381552797961],
                 [146.0369496666425, 261.2407284653631], [237.47876457530091, 223.7126401544219],
                 [202.28905650003946, 164.64306046915422], [277.2234416699363, 195.7718148881111],
                 [107.33827248315202, 34.72071569922639], [182.62301709068336, 235.29335985963388],
                 [3.611036284297675, 83.92057090424217], [221.33036793953298, 200.2061349656116],
                 [217.56567113826276, 155.73014603908308], [88.08883858221664, 209.79535009887528],
                 [155.49061896108842, 24.497018178704955], [50.42035839306407, 184.25757194105802],
                 [71.6437982227808, 184.9457620526441], [94.28227362925041, 101.80780110742043],
                 [14.246945723801774, 154.55827967291748], [121.90620606995161, 64.36486948073617],
                 [112.48006693246538, 238.43290512163108], [160.53257841331313, 31.08879176379865],
                 [247.74440929406356, 219.3903775085534], [273.36882698279527, 172.08011506263915],
                 [55.594608554652304, 95.82012681294651], [15.386425375549095, 15.5886895464193],
                 [105.08683869305916, 166.016513916169], [95.90471396258783, 1.3216962712470859]]  # 传感器位置的列表


uav_coor_list = [[120, 160], [250, 225], [10, 280], [275, 50], [40, 23]]
for i in range(0, 200):  # 1000个传感器
    app_list.append(Appliances(i))
    app_list[i].setcoor(app_coor_list[i])

for i in range(0, 5):  # 5个无人机
    uav_list.append(UAV(i))
    if i == 0:
        uav_list[i].coordinates_x_y = [120, 160]
    elif i == 1:
        uav_list[i].coordinates_x_y = [250, 225]
    elif i == 2:
        uav_list[i].coordinates_x_y = [10, 280]
    elif i == 3:
        uav_list[i].coordinates_x_y = [275, 50]
    elif i == 4:
        uav_list[i].coordinates_x_y = [40, 23]

UAV_R = 23

# 所有障碍物的位置
x_obstacle = [105, 230, 200, 35]
y_obstacle = [210, 260, 100, 50]
obstacle_R = [42, 32, 36, 46]


def orbit(solution):
    fit = 0
    # 根据角度计算无人机的坐标
    uav_coor_temp = []
    for i in range(0, len(solution)):
        uav_coor_temp.append(uav_list[i].get_next_coordinates_x_y(solution[i]))
    # 根据无人机坐标判断是否满足限制条件，若不满足适应度减，若满足适应度加无人机覆盖数
    for i in range(0, len(uav_coor_temp)):
        # 1、判断无人机是否碰撞障碍
        for j in range(0, len(x_obstacle)):
            if (uav_coor_temp[i][0] - x_obstacle[j]) ** 2 + (uav_coor_temp[i][1] - y_obstacle[j]) ** 2 <= \
                    obstacle_R[j] ** 2:
                fit -= 500
        # 2、判断无人机之间的覆盖范围是否相交
        for j in range(0, len(uav_coor)):
            flag = False
            if ((uav_coor_temp[i][0] - uav_coor[j][0]) ** 2 + (
                    uav_coor_temp[i][1] - uav_coor[j][1]) ** 2 < 4 * UAV_R ** 2):
                flag = True
            if (j - i) % 5 == 0 and flag:
                fit -= 30
            elif flag:
                fit -= 50
        # 3、无人机坐标不能超出边界
        if (uav_coor_temp[i][0] < 0 or
                uav_coor_temp[i][0] > 300 or
                uav_coor_temp[i][1] < 0 or
                uav_coor_temp[i][1] > 300):
            fit -= 1000
        for j in range(0, len(app_list)):
            if (app_list[j].coordinates_x_y[0] - uav_coor_temp[i][0]) ** 2 + (
                    app_list[j].coordinates_x_y[1] - uav_coor_temp[i][1]) ** 2 <= UAV_R ** 2:
                fit += 4
    return fit

if __name__ == '__main__':

    problem_dict = {
        "obj_func": orbit,
        "bounds": FloatVar(lb=[0, 0, 0, 0, 0], ub=[360, 360, 360, 360, 360]),
        # "bounds": FloatVar(lb=[-10000], ub=[10000]),
        "minmax": "max",
    }

    cover_list = [[], [], [], [], []]  # 无人机覆盖传感器集合
    cover_list_ = [[], [], [], [], []]  #
    uav_coor = []
    for i in range(0, 5):
        for id_item in uav_list[i].cover(app_list):
            cover_list[i].append(id_item)

    for step in range(0, 24):
        optimizer = SCA.DevSCA(epoch=100, pop_size=50)
        # optimizer = GA.BaseGA(epoch=100, pop_size=50, pc=0.95, pm=0.2)
        optimizer.solve(problem_dict)
        # print('最优值：{}适应度：{}'.format(optimizer.g_best.solution, optimizer.g_best.target.fitness))
        for i in range(5):
            theta = optimizer.g_best.solution[i]  # 角度由算法确定
            uav_list[i].reset_coordinates_x_y(theta)
            uav_coor.append(uav_list[i].coordinates_x_y.copy())
            for id_item in uav_list[i].cover(app_list):
                cover_list[i].append(id_item)
            fit_last = len(cover_list)
            uav_coor_list.append(uav_list[i].coordinates_x_y.copy())
    # 将cover_list中重复的数据去掉，方便之后统计
    cover = []
    for i in range(0, 5):
        for id in cover_list[i]:
            cover.append(id)
    cover = list(set(cover))
    non_cover_list = []  # 没被无人机覆盖的传感器列表
    for i in range(0, len(app_list)):
        if not app_list[i].flag:
            non_cover_list.append(app_list[i].id)
    cover_num = len(cover)
    print(len(app_list))
    print('无人机覆盖率：{}'.format(cover_num / len(app_list)))
    x = []
    y = []
    x_u0 = []
    y_u0 = []
    x_u1 = []
    y_u1 = []
    x_u2 = []
    y_u2 = []
    x_u3 = []
    y_u3 = []
    x_u4 = []
    y_u4 = []
    obstacle_R = [42, 32, 36, 46]
    size_obstacle = [3024, 2304, 2592, 3312]  # 一米72
    for i in range(len(app_coor_list)):
        x.append(app_coor_list[i][0])
        y.append(app_coor_list[i][1])
    for i in range(0, len(uav_coor_list) - 4, 5):
        x_u0.append(uav_coor_list[i][0])
        y_u0.append(uav_coor_list[i][1])
        x_u1.append(uav_coor_list[i + 1][0])
        y_u1.append(uav_coor_list[i + 1][1])
        x_u2.append(uav_coor_list[i + 2][0])
        y_u2.append(uav_coor_list[i + 2][1])
        x_u3.append(uav_coor_list[i + 3][0])
        y_u3.append(uav_coor_list[i + 3][1])
        x_u4.append(uav_coor_list[i + 4][0])
        y_u4.append(uav_coor_list[i + 4][1])

    # 第一个图
    fig = plt.figure(1)
    ax = fig.add_subplot(111)
    # 绘制散点图
    plt.scatter(x, y, s=5)
    plt.scatter(x_u0, y_u0, s=1080, alpha=0.1)
    plt.scatter(x_u1, y_u1, s=1080, alpha=0.1)
    plt.scatter(x_u2, y_u2, s=1080, alpha=0.1)
    plt.scatter(x_u3, y_u3, s=1080, alpha=0.1)
    plt.scatter(x_u4, y_u4, s=1080, alpha=0.1)
    plt.plot(x_u0, y_u0, marker='<')
    plt.plot(x_u1, y_u1, marker='s')
    plt.plot(x_u2, y_u2, marker='*')
    plt.plot(x_u3, y_u3, marker='d')
    plt.plot(x_u4, y_u4, marker='x')
    plt.scatter(x_obstacle, y_obstacle, s=size_obstacle, alpha=0.5, c='black')
    # 设置标题和坐标轴标签
    plt.title('Simple Scatter Plot')
    plt.xlabel('X')
    plt.ylabel('Y')
    ax.set_aspect('equal', adjustable='box')

    # 显示图形
    plt.show()